#include "../Device/cmd.h"

#include "main.h"

#define MOTOR_OUT_MAX (999)

UART_HandleTypeDef *huart = NULL;
DMA_HandleTypeDef *hdma_usart_rx = NULL;
DMA_HandleTypeDef *hdma_usart_tx = NULL;

extern float angle_setpoint, speed_setpoint, speed_max, yaw_setpoint;
extern float angle_p, angle_i, angle_d, speed_p, speed_i, speed_d, turn_p;
extern float machine_zero_roll;

struct {
  volatile uint8_t len;
  volatile uint8_t flag;
  uint8_t buffer[BUFFER_SIZE];
} Uart;

void Uart_Init(UART_HandleTypeDef *handle, DMA_HandleTypeDef *dma_tx,
               DMA_HandleTypeDef *dma_rx) {
  huart = handle;
  hdma_usart_rx = dma_rx;
  hdma_usart_tx = dma_tx;
  __HAL_UART_ENABLE_IT(huart, UART_IT_IDLE);
  HAL_UART_Receive_DMA(huart, Uart.buffer, BUFFER_SIZE);
}

uint8_t *Uart_GetDataAddr() { return Uart.buffer; }
uint8_t Uart_GetDataLen() { return Uart.len; }

uint8_t Uart_SendDMA(uint8_t *buf, uint8_t len) {
  return HAL_UART_Transmit_DMA(huart, buf, len);
}

uint8_t Uart_CheckFlag() {
  if (Uart.flag) {
    Uart.flag = 0;
    Uart_StartDMA();
    return 1;
  } else
    return 0;
}

uint8_t Uart_StartDMA() {
  return HAL_UART_Receive_DMA(huart, Uart.buffer, BUFFER_SIZE);
}

void Uart_IRQHandler() {
  if ((__HAL_UART_GET_FLAG(huart, UART_FLAG_IDLE) != RESET)) {
    __HAL_UART_CLEAR_IDLEFLAG(huart);
    HAL_UART_DMAStop(huart);
    Uart.len = BUFFER_SIZE - __HAL_DMA_GET_COUNTER(hdma_usart_rx);
    Uart.flag = 1;
  }
}

void Prase_CMD(uint8_t *data, uint8_t len) {
  if (data[4] != 'l') return;
  int8_t *tmp = (int8_t *)&data[1];
  int16_t angle_tmp;
  switch (data[0]) {
    case 'r': /* 角度控制命令(右) */
      angle_tmp = ((tmp[0] - '0') * 10 + (tmp[1] - '0')) * 10 + (tmp[2] - '0') +
                  yaw_setpoint;
      if (angle_tmp >= 360) angle_tmp -= 360;
      yaw_setpoint = angle_tmp;
      break;
    case 'l': /* 角度控制命令(左) */
      angle_tmp = yaw_setpoint - (((tmp[0] - '0') * 10 + (tmp[1] - '0')) * 10 +
                                  (tmp[2] - '0'));
      if (angle_tmp < 0) angle_tmp += 360;
      yaw_setpoint = angle_tmp;
      break;
    case 's': /* 调节速度 */
      speed_max =
          (float)(((tmp[0] - '0') * 10 + (tmp[1] - '0')) * 10 + (tmp[2] - '0'));
      if (speed_max > MOTOR_OUT_MAX) speed_max = MOTOR_OUT_MAX;
      break;
    case 'p': /* 调节pid */
      angle_p = (float)(((tmp[0] - '0') * 10 + (tmp[1] - '0')) * 10 +
                        (tmp[2] - '0')) /
                10.0f;
      break;
    case 'i': /* 调节pid */
      angle_i = (float)(((tmp[0] - '0') * 10 + (tmp[1] - '0')) * 10 +
                        (tmp[2] - '0')) /
                10.0f;
      break;
    case 'd':
      angle_d = (float)(((tmp[0] - '0') * 10 + (tmp[1] - '0')) * 10 +
                        (tmp[2] - '0')) /
                10.0f;
      break;
    case 'P':
      speed_p = (float)(((tmp[0] - '0') * 10 + (tmp[1] - '0')) * 10 +
                        (tmp[2] - '0')) /
                10.0f;
      break;
    case 'I':
      speed_i = (float)(((tmp[0] - '0') * 10 + (tmp[1] - '0')) * 10 +
                        (tmp[2] - '0')) /
                100.0f;
      break;
    case 't':
      turn_p = (float)(((tmp[0] - '0') * 10 + (tmp[1] - '0')) * 10 +
                       (tmp[2] - '0')) /
               10.0f;
      break;
    case 'm':
      machine_zero_roll = (float)(((tmp[0] - '0') * 10 + (tmp[1] - '0')) * 10 +
                                  (tmp[2] - '0')) /
                          100.0f;
      break;
    default:
      return;
  }
}
